#include "xd_control.h"
#include "xd_message.h"
#include "rtthread.h"
#include "main.h"
#include <stdbool.h>
#include "gqueue.h"
#include "zy_tools.h"
#include "leds.h"
#include "zy_can.h"
#include "grb_control.h"
rt_sem_t semXdRcv;
gqueue_t qxdSend;
gqueue_t qxdRcv;
uint8_t xdSendBuf[XD_PROCESS_MAXLEN*2] = {0};
uint8_t xdRcvBuf[XD_PROCESS_MAXLEN*2] = {0};
uint8_t xdProcessBuf[XD_PROCESS_MAXLEN] = {0};
void xd_Rcv_Sem(void)
{
    if(semXdRcv != RT_NULL)
    {
        rt_sem_release(semXdRcv);
    }
}
extern uint16_t CDC_Send(void const* buf,uint32_t bs);
void XD_Thread_Entry(void* parameter)
{
    uint32_t xdRcvCount = 0;
    xd_message_t msg;
    int headerIndex = 0,discardLen = 0;
    bool frameFound = false;
    gqueue_init(&qxdRcv,xdRcvBuf,sizeof(xdRcvBuf),sizeof(uint8_t));
    semXdRcv = rt_sem_create("xd rcv",0,RT_IPC_FLAG_FIFO);
    xd_message_set_send_func(CDC_Send);

    if(CAN_Open(2,1000))
    {
        rt_kprintf("CAN2_Open failed\r\n");
    }
    rt_thread_delay(500);
    car_front_back_strip_control(0,1,50);
    while(1)
    {
        rt_sem_take(semXdRcv,RT_WAITING_FOREVER);
        bool processOver = false;
        do
        {
            //数据处理
            //tag[0xAA,0x55](2) data_length(1) type(1) data(n) crc16(2)
            xdRcvCount++;
            if(xdRcvCount > 50){
                xdRcvCount = 0;
                blinkLed1(50, 1);
            }
            frameFound = false;
            memset(xdProcessBuf,0,XD_PROCESS_MAXLEN);
            size_t peekLen = 0;
            gqueue_peek_multiple_most(&qxdRcv,xdProcessBuf,XD_PROCESS_MAXLEN,&peekLen);
            if(peekLen >= HEADER_LENGTH && peekLen < XD_PROCESS_MAXLEN)
            {

                for(headerIndex = 0;headerIndex < peekLen-1;headerIndex++)
                {
                    if(xdProcessBuf[headerIndex] == 0xAA && xdProcessBuf[headerIndex+1] == 0x55)
                    {

                        if(headerIndex + HEADER_LENGTH > peekLen)       // 找到了头但是不够一帧了
                        {
                            peekLen = headerIndex;                      // 重新赋值peekLen，在下面的else丢掉之前的数据
                        }
                        else
                        {
                            frameFound = true;
                        }
                        break;
                    }
                }
            }
            else
            {
                processOver = true;     // 数据不足
                peekLen = 0;            // 不丢弃数据，结束循环
            }

            if(frameFound)
            {
                int type = xdProcessBuf[headerIndex+3];
                int data_len = xdProcessBuf[headerIndex+2];
                int packLen = HEADER_LENGTH + data_len;
                if(headerIndex + packLen <= peekLen)        // 包已接收完
                {
                    uint16_t calcCrc16 = CRC16(&xdProcessBuf[headerIndex],packLen-2);
                    int crcIndex = headerIndex + packLen - 2;
                    uint16_t packCrc16 = xdProcessBuf[crcIndex] | (xdProcessBuf[crcIndex + 1] << 8);
                    if(calcCrc16 == packCrc16)
                    {
                        msg.type = type;
                        msg.data_len = data_len;
                        msg.data = &xdProcessBuf[headerIndex+4];
                        XD_Message_Received(&msg);
                        discardLen = headerIndex + packLen;
                    }
                    else        // 数据有问题
                    {
                        discardLen = headerIndex + 2;   // 不是一个有效的包，只把“zy”丢弃
                    }

                }
                else        // 包超长
                {
                    if(headerIndex > 0)     // 如果header发现的位置不是0，先丢弃前面的数据再处理
                    {
                        discardLen = headerIndex;
                    }
                    else
                    {
                        if(packLen >= sizeof(xdProcessBuf)) // 处理不了这么长数
                        {
                            discardLen = headerIndex + 2;       // 把header丢弃
                        }
                        else    // 数据没接收完，丢弃没用的数据等待下次处理
                        {
                            processOver = true;
                            discardLen = headerIndex;
                        }
                    }

                }
            }
            else
            {
                discardLen = peekLen;
            }
            gqueue_discard_multiple(&qxdRcv,discardLen);        // 删除已处理或者无效的数据

        }while(!processOver);
    }
}
